global uLINK
global ID

RobotWaistWidth = 0.2;%腰长
RobotThighLength = 0.5;%大腿长
RobotShankLength = 0.5;%小腿长

ID.NONE = 0;
ID.BODY = 1;
ID.R_LEG_J0 = 2;
ID.R_LEG_J1 = 3;
ID.R_LEG_J2 = 4;
ID.R_LEG_J3 = 5;
ID.R_LEG_J4 = 6;
ID.R_LEG_J5 = 7;

ID.L_LEG_J0 = 8;
ID.L_LEG_J1 = 9;
ID.L_LEG_J2 = 10;
ID.L_LEG_J3 = 11;
ID.L_LEG_J4 = 12;
ID.L_LEG_J5 = 13;
%质量和质心位置不明白
uLINK(ID.BODY).name = 'BODY';
uLINK(ID.BODY).mother = 0;
uLINK(ID.BODY).child = ID.R_LEG_J0;
uLINK(ID.BODY).sister = 0;
uLINK(ID.BODY).p = [0 0 0]';%在世界坐标系中的位置
uLINK(ID.BODY).R = [1,0,0; 0,1,0; 0,0,1];%在世界坐标系中的姿态
uLINK(ID.BODY).b = [0 0 0]';%母连杆的相对距离
uLINK(ID.BODY).a = [0 0 0]';%关节轴矢量
uLINK(ID.BODY).q = 0;%关节角度
uLINK(ID.BODY).dq = 0;%关节角速度
uLINK(ID.BODY).ddq = 0;%关节角加速度
uLINK(ID.BODY).m = 20;%质量
uLINK(ID.BODY).c = [0,0,0.2]';%质心位置

uLINK(ID.R_LEG_J0).name = 'R_LEG_J0';
uLINK(ID.R_LEG_J0).mother = ID.BODY;
uLINK(ID.R_LEG_J0).child = ID.R_LEG_J1;
uLINK(ID.R_LEG_J0).sister = 8;
uLINK(ID.R_LEG_J0).p = [0 0 0]';
uLINK(ID.R_LEG_J0).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J0).b = [0 -RobotWaistWidth 0]';
uLINK(ID.R_LEG_J0).a = [0 0 1]';
uLINK(ID.R_LEG_J0).q = 0;
uLINK(ID.R_LEG_J0).dq = 0;%关节角速度
uLINK(ID.R_LEG_J0).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J0).m = 0;%质量
uLINK(ID.R_LEG_J0).c = [0,0,0]';%质心位置

uLINK(ID.R_LEG_J1).name = 'R_LEG_J1';
uLINK(ID.R_LEG_J1).mother = ID.R_LEG_J0;
uLINK(ID.R_LEG_J1).child = ID.R_LEG_J2;
uLINK(ID.R_LEG_J1).sister = 0;
uLINK(ID.R_LEG_J1).p = [0 0 0]';
uLINK(ID.R_LEG_J1).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J1).b = [0 0 0]';
uLINK(ID.R_LEG_J1).a = [1 0 0]';
uLINK(ID.R_LEG_J1).q = 0;
uLINK(ID.R_LEG_J1).dq = 0;%关节角速度
uLINK(ID.R_LEG_J1).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J1).m = 0;%质量
uLINK(ID.R_LEG_J1).c = [0,0,0]';%质心位置

uLINK(ID.R_LEG_J2).name = 'R_LEG_J2';
uLINK(ID.R_LEG_J2).mother = ID.R_LEG_J1;
uLINK(ID.R_LEG_J2).child = ID.R_LEG_J3;
uLINK(ID.R_LEG_J2).sister = 0;
uLINK(ID.R_LEG_J2).p = [0 0 0]';
uLINK(ID.R_LEG_J2).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J2).b = [0 0 0]';
uLINK(ID.R_LEG_J2).a = [0 1 0]';
uLINK(ID.R_LEG_J2).q = 0;
uLINK(ID.R_LEG_J2).dq = 0;%关节角速度
uLINK(ID.R_LEG_J2).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J2).m = 12;%质量
uLINK(ID.R_LEG_J2).c = [0,0,0.2]';%质心位置

uLINK(ID.R_LEG_J3).name = 'R_LEG_J3';
uLINK(ID.R_LEG_J3).mother = ID.R_LEG_J2;
uLINK(ID.R_LEG_J3).child = ID.R_LEG_J4;
uLINK(ID.R_LEG_J3).sister = 0;
uLINK(ID.R_LEG_J3).p = [0 0 0]';
uLINK(ID.R_LEG_J3).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J3).b = [0 0 -RobotThighLength]';
uLINK(ID.R_LEG_J3).a = [0 1 0]';
uLINK(ID.R_LEG_J3).q = 0;
uLINK(ID.R_LEG_J3).dq = 0;%关节角速度
uLINK(ID.R_LEG_J3).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J3).m = 20;%质量
uLINK(ID.R_LEG_J3).c = [0,0,0.2]';%质心位置

uLINK(ID.R_LEG_J4).name = 'R_LEG_J4';
uLINK(ID.R_LEG_J4).mother = ID.R_LEG_J3;
uLINK(ID.R_LEG_J4).child = ID.R_LEG_J5;
uLINK(ID.R_LEG_J4).sister = 0;
uLINK(ID.R_LEG_J4).p = [0 0 0]';
uLINK(ID.R_LEG_J4).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J4).b = [0 0 -RobotShankLength]';
uLINK(ID.R_LEG_J4).a = [0 1 0]';
uLINK(ID.R_LEG_J4).q = 0;
uLINK(ID.R_LEG_J4).dq = 0;%关节角速度
uLINK(ID.R_LEG_J4).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J4).m = 20;%质量
uLINK(ID.R_LEG_J4).c = [0,0,0.2]';%质心位置

uLINK(ID.R_LEG_J5).name = 'R_LEG_J5';
uLINK(ID.R_LEG_J5).mother = ID.R_LEG_J4;
uLINK(ID.R_LEG_J5).child = 0;
uLINK(ID.R_LEG_J5).sister = 0;
uLINK(ID.R_LEG_J5).p = [0 0 0]';
uLINK(ID.R_LEG_J5).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.R_LEG_J5).b = [0 0 0]';
uLINK(ID.R_LEG_J5).a = [1 0 0]';
uLINK(ID.R_LEG_J5).q = 0;
uLINK(ID.R_LEG_J5).dq = 0;%关节角速度
uLINK(ID.R_LEG_J5).ddq = 0;%关节角加速度
uLINK(ID.R_LEG_J5).m = 20;%质量
uLINK(ID.R_LEG_J5).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J0).name = 'L_LEG_J0';
uLINK(ID.L_LEG_J0).mother = ID.BODY;
uLINK(ID.L_LEG_J0).child = ID.L_LEG_J1;
uLINK(ID.L_LEG_J0).sister = 0;
uLINK(ID.L_LEG_J0).p = [0 0 0]';
uLINK(ID.L_LEG_J0).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J0).b = [0 RobotWaistWidth 0]';
uLINK(ID.L_LEG_J0).a = [0 0 1]';
uLINK(ID.L_LEG_J0).q = 0;
uLINK(ID.L_LEG_J0).dq = 0;%关节角速度
uLINK(ID.L_LEG_J0).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J0).m = 20;%质量
uLINK(ID.L_LEG_J0).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J1).name = 'L_LEG_J1';
uLINK(ID.L_LEG_J1).mother = ID.L_LEG_J0;
uLINK(ID.L_LEG_J1).child = ID.L_LEG_J2;
uLINK(ID.L_LEG_J1).sister = 0;
uLINK(ID.L_LEG_J1).p = [0 0 0]';
uLINK(ID.L_LEG_J1).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J1).b = [0 0 0]';
uLINK(ID.L_LEG_J1).a = [1 0 0]';
uLINK(ID.L_LEG_J1).q = 0;
uLINK(ID.L_LEG_J1).dq = 0;%关节角速度
uLINK(ID.L_LEG_J1).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J1).m = 20;%质量
uLINK(ID.L_LEG_J1).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J2).name = 'L_LEG_J2';
uLINK(ID.L_LEG_J2).mother = ID.L_LEG_J1;
uLINK(ID.L_LEG_J2).child = ID.L_LEG_J3;
uLINK(ID.L_LEG_J2).sister = 0;
uLINK(ID.L_LEG_J2).p = [0 0 0]';
uLINK(ID.L_LEG_J2).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J2).b = [0 0 0]';
uLINK(ID.L_LEG_J2).a = [0 1 0]';
uLINK(ID.L_LEG_J2).q = 0;
uLINK(ID.L_LEG_J2).dq = 0;%关节角速度
uLINK(ID.L_LEG_J2).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J2).m = 20;%质量
uLINK(ID.L_LEG_J2).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J3).name = 'L_LEG_J3';
uLINK(ID.L_LEG_J3).mother = ID.L_LEG_J2;
uLINK(ID.L_LEG_J3).child = ID.L_LEG_J4;
uLINK(ID.L_LEG_J3).sister = 0;
uLINK(ID.L_LEG_J3).p = [0 0 0]';
uLINK(ID.L_LEG_J3).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J3).b = [0 0 -RobotThighLength]';
uLINK(ID.L_LEG_J3).a = [0 1 0]';
uLINK(ID.L_LEG_J3).q = 0;
uLINK(ID.L_LEG_J3).dq = 0;%关节角速度
uLINK(ID.L_LEG_J3).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J3).m = 20;%质量
uLINK(ID.L_LEG_J3).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J4).name = 'L_LEG_J4';
uLINK(ID.L_LEG_J4).mother = ID.L_LEG_J3;
uLINK(ID.L_LEG_J4).child = ID.L_LEG_J5;
uLINK(ID.L_LEG_J4).sister = 0;
uLINK(ID.L_LEG_J4).p = [0 0 0]';
uLINK(ID.L_LEG_J4).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J4).b = [0 0 -RobotShankLength]';
uLINK(ID.L_LEG_J4).a = [0 1 0]';
uLINK(ID.L_LEG_J4).q = 0;
uLINK(ID.L_LEG_J4).dq = 0;%关节角速度
uLINK(ID.L_LEG_J4).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J4).m = 20;%质量
uLINK(ID.L_LEG_J4).c = [0,0,0.2]';%质心位置

uLINK(ID.L_LEG_J5).name = 'L_LEG_J5';
uLINK(ID.L_LEG_J5).mother = ID.L_LEG_J4;
uLINK(ID.L_LEG_J5).child = 0;
uLINK(ID.L_LEG_J5).sister = 0;
uLINK(ID.L_LEG_J5).p = [0 0 0]';
uLINK(ID.L_LEG_J5).R = [1,0,0; 0,1,0; 0,0,1];
uLINK(ID.L_LEG_J5).b = [0 0 0]';
uLINK(ID.L_LEG_J5).a = [1 0 0]';
uLINK(ID.L_LEG_J5).q = 0;
uLINK(ID.L_LEG_J5).dq = 0;%关节角速度
uLINK(ID.L_LEG_J5).ddq = 0;%关节角加速度
uLINK(ID.L_LEG_J5).m = 20;%质量
uLINK(ID.L_LEG_J5).c = [0,0,0.2]';%质心位置
